#include <iostream>
#include "hrCore_mod.hpp"
#include <alcommon/albroker.h>
#include "aleigen.hpp"


//testing commit
//testing messsage
HRCoreMod::HRCoreMod(boost::shared_ptr<AL::ALBroker> broker,
                 const std::string& name)
: AL::ALModule(broker, name)
{
    done = false;
    // Describe the module here. This will appear on the webpage
    setModuleDescription("libHRCore manager module ");
    /**
      * Define callable methods with their descriptions:
      * This makes the method available to other cpp modules
      * and to python.
      * The name given will be the one visible from outside the module.
      * This method has no parameters or return value to describe
      * functionName(<method_name>, <class_name>, <method_description>);
      * BIND_METHOD(<method_reference>);
      */


    // BIND METHODS
    functionName("isDone", getName(), "return true if done loading robot");
    BIND_METHOD(HRCoreMod::isDone);

    functionName("getNumberOfBodies", getName(), "return number of bodies");
    BIND_METHOD(HRCoreMod::getNumberOfBodies);

    functionName("getDoF", getName(), "return number of deegres of freedom");
    BIND_METHOD(HRCoreMod::getDoF);

    functionName("getNameMod", getName(), "return module name");
    BIND_METHOD(HRCoreMod::getNameMod);

    functionName("getLevelMod", getName(), "return module level");
    BIND_METHOD(HRCoreMod::getLevelMod);

    /**
      * setReturn(<return_name>, <return_description>);
      * This enables to document the return of the method.
      * It is not compulsory to write this line.
      */


    functionName("getMatrixMod", getName(), "return module matrix in ALValue(Eigen)");
    setReturn("matrixMod","The matrix in ALValue(Eigen)");
    BIND_METHOD(HRCoreMod::getMatrixMod);


    /**
     * addParam(<attribut_name>, <attribut_descrption>);
     * This enables to document the parameters of the method.
     * It is not compulsory to write this line.
     */


    functionName("getBodyPosition",getName(), "return body position in ALValue(Eigen)");
    addParam("bodyId","The body id to compute position");
    setReturn("alPosition","ALPosition in ALValue(Eigen)");
    BIND_METHOD(HRCoreMod::getBodyPosition);



    world = World();
    sFile =  "/home/nao/projects/hrData/Nao_blue_noninertial.xbot";
    MultiBodyID = world.getRobotsVector()->size();


    nameMod = "My name is libHRCore";
    levelMod = 1;
    matrixMod.resize(2,2);
    matrixMod << 1,2,3,4;
}

HRCoreMod::~HRCoreMod()
{
}

void HRCoreMod::init()
{

    /**
     * Init is called just after construction.
     * Do something or not
     */
    world.loadMultiBody(sFile,MultiBodyID,hrCore::NAO);
    done = true;
}

AL::ALValue HRCoreMod::getMatrixMod(){
    return castEigenToAL(matrixMod);
}

AL::ALValue HRCoreMod::getBodyPosition(const int & bodyId){
    Eigen::Vector3f position =  world.getRobot(MultiBodyID)->getRelativeBodyPosition(bodyId).cast<float>();
    //AL::ALValue val;
    AL::ALValue val = castEigenToAL(position);
    return val;
}

int HRCoreMod::getNumberOfBodies(){
    return world.getRobot(MultiBodyID)->getNumberOfBodies();
}
int HRCoreMod::getDoF(){
    return world.getRobot(MultiBodyID)->getDoF();
}


